#include <iostream.h>
#include "math.h"
#include <taskLib.h>

#include "AxisCamera.h" 
#include "BaeUtilities.h"
#include "FrcError.h"
#include "PID.h"
#include "PCVideoServer.h"
#include "TrackAPI.h" 
#include "PIDOutput.h"
#include "PIDSource.h"
#include "Gyro.h"
#include "WPILib.h"

#define MIN_PARTICLE_TO_IMAGE_PERCENT 0.01		// target is too small
#define MAX_PARTICLE_TO_IMAGE_PERCENT 100.0		// target is too close

/**
 * This is a demo program showing the use of the RobotBase class.
 * The SimpleRobot class is the base of a robot application that will automatically call your
 * Autonomous and OperatorControl methods at the right time as controlled by the switches on
 * the driver station or the field controls.
 */
class RobotDemo : public SimpleRobot
{
	RobotDrive *myRobot; // robot drive system
	Joystick *stick1, *stick2, *stick3, *stick4; // three joysticks
	DriverStation *ds; // driver station
	Jaguar *one;
	Jaguar *two;
	Jaguar *three;
	Jaguar *four;
	Timer *myTimer, *Camtimer;
	PIDController *PID;
	Gyro *myAccel;
	Gyro *myGyro;
	AnalogChannel *myPot;
	float x1,y1,z1,twist1,x2,y2,y3;
	TrackingThreshold tdata; 	// image data for tracking
	
public:
	RobotDemo(void)
	{
		one = new Jaguar(1) ;
		two = new Jaguar(2);
		three = new Jaguar(3); 
		four = new Jaguar(4);	
		ds = DriverStation::GetInstance();
		myRobot = new RobotDrive(one,three); // create robot drive base
		stick1 = new Joystick(1); // create the joysticks
		stick2 = new Joystick(2); // create the joysticks
		stick3 = new Joystick(3); // create the joysticks
		stick4 = new Joystick(4); // for globe on turret
		GetWatchdog().SetExpiration(100);
		myTimer = new Timer();
		Camtimer = new Timer();
		PID = new PIDController(1.0,0.0,0.0);
		myAccel = new Gyro(2);
		myGyro = new Gyro(1);
		
		//myAccel->m_analog->SetAccumulatorCenter(304);
		//myAccel->m_analog->SetAccumulatorDeadband(10);
		//myGyro->m_analog->SetAccumulatorCenter(491);
		//myGyro->m_analog->SetAccumulatorDeadband(11);
		
		myPot = new AnalogChannel(3);
		x1 = 0;
		y1 = 0;
		z1 = 0;
		twist1 = 0;
		x2 = 0;
		y2 = 0;
		y3 = 0;
		
		SetDebugFlag(DEBUG_SCREEN_ONLY);
		
		/* start the CameraTask	 */
		if (StartCameraTask(20, 0, k160x120, ROT_0) == -1) {
			dprintf( LOG_ERROR,"Failed to spawn camera task; Error code %s", 
					GetVisionErrorText(GetLastVisionError()) );
		}
		printf("%s\n",GetVisionErrorText(GetLastVisionError()));
		// values for tracking a target - may need tweaking in your environment
		// see TrackAPI.cpp for threshold values
		
		tdata = GetTrackingData(PINK, FLUORESCENT);
		tdata.luminance.minValue = 120;
		tdata.luminance.maxValue = 180;
		tdata.hue.minValue = 200;
		tdata.hue.maxValue = 255;
		tdata.saturation.minValue = 60;
		tdata.saturation.maxValue = 160;

		myGyro->SetSensitivity(0.007);
		myAccel->SetSensitivity(0.0306);
	}
	
	void Autonomous(void)
	{		
		//PCVideoServer pc;
		
		GetWatchdog().Feed();
		
		Camtimer->Reset();
		Camtimer->Start();
		
		while (IsAutonomous()) 
		{
			GetWatchdog().Feed();
			UpdateCamLED(true);
			
			//printf("Gyro: %f, Accel: %f Pot: %f\n",myGyro->GetAngle(),myAccel->GetAngle(),myPot->GetVoltage());
		}

		myRobot->Drive(0.0, 0.0); // stop robot
		two->Set(0.0);

	}

	void OperatorControl(void)
	{
		//PCVideoServer pc;
		
		const double period = 0.1;
		const double pulse = 0.075;
		
		char drivemode = 0;
		
		GetWatchdog().SetEnabled(true); 
		
		myTimer->Reset();
		myTimer->Start();
		Camtimer->Reset();
		Camtimer->Start();
		while (IsOperatorControl())
		{
			GetWatchdog().Feed();

			
			UpdateCamLED(stick4->GetRawButton(5));
			ShowActivity("Gyro: %f, Accel: %f Pot: %f",myGyro->GetAngle(),myAccel->GetAngle(),myPot->GetVoltage());
			
			//printf("Timer: %f\n", myTimer->Get());
			
			drivemode = ((char)ds->GetDigitalIn(1) << 1) + (char)ds->GetDigitalIn(2);
	
			//printf("DriveMode: %d\n", drivemode);
			
			if(!(stick1->GetRawButton(6) ||
					stick2->GetTrigger() ||
					stick3->GetTrigger()))
			{
				if(myTimer->Get() > period)
				{
					myTimer->Reset();
				}
				if(myTimer->Get() > pulse)
				{
					myRobot->ArcadeDrive(0,0,false);
				}
				else
				{
					TeleOpDrive(drivemode);		
				}
			}
			else
			{
				TeleOpDrive(drivemode);
			}
		}
	}
	
	void TeleOpDrive(char drivemode)
	{
		x1=stick1->GetX();
		y1=stick1->GetY();
		z1=stick1->GetZ();
		twist1=stick1->GetTwist();
		x2=stick2->GetX();
		y2=stick2->GetY();
		y3=stick3->GetY();
		
		switch(drivemode)
		{
		case 0:
			myRobot->ArcadeDrive(y1,z1,false);
			break;
		case 1:
			myRobot->TankDrive(y1,twist1);
			break;
		case 2:
			myRobot->ArcadeDrive(y2,x2,false);
			break;
		case 3:
			myRobot->TankDrive(y2,y3);
			break;
		}
	}
	void UpdateCamLED(bool tracking)
	{
		static unsigned char camLED = 0;
		static unsigned char RangeLED = 0;
		static ParticleAnalysisReport par;
		
		if(Camtimer->Get() > 0.025)
		{
			Camtimer->Reset();
			if (FindColor(IMAQ_HSL, &tdata.hue, &tdata.saturation, &tdata.luminance, &par)
				&& par.particleToImagePercent <= MAX_PARTICLE_TO_IMAGE_PERCENT
				&& par.particleToImagePercent >= MIN_PARTICLE_TO_IMAGE_PERCENT)
			{
		
				if ((myPot->GetVoltage() > 3.70) && (par.center_mass_x_normalized > 0))
				{
					two->Set(0);
				}
				else if ((myPot->GetVoltage() < 1.17) && (par.center_mass_x_normalized < 0))
				{
					two->Set(0);
				}
				else
				{
					if(tracking)
					{
						two->Set(par.center_mass_x_normalized);
					}
					else
					{
						two->Set(stick4->GetX());
					}
				}
		
				ShowActivity("Colour: %s\tX: %f\tY: %f\tSize: %f\tFound: YES", tdata.name, par.center_mass_x_normalized, par.center_mass_y_normalized, par.particleToImagePercent);
				
					
				if (par.center_mass_x_normalized < -0.6)
					camLED = 6;
				else if (par.center_mass_x_normalized < -0.2)
					camLED = 5;
				else if (par.center_mass_x_normalized < 0.2)
					camLED = 4;
				else if (par.center_mass_x_normalized < 0.6)
					camLED = 3;
				else
					camLED = 2;
				
				if (par.particleToImagePercent > 8.0)
					RangeLED = 6;
				else if (par.particleToImagePercent > 1.5)
					RangeLED = 5;
				else if (par.particleToImagePercent > 0.5)
					RangeLED = 4;
				else if (par.particleToImagePercent > 0.05)
					RangeLED = 3;
				else
					RangeLED = 2;
			
			} 
			else 
			{
				ShowActivity("Colour: %s\tX: %f\tY: %f\tSize: %f\tFound: NO", tdata.name, par.center_mass_x_normalized, par.center_mass_y_normalized, par.particleToImagePercent);
				two->Set(stick4->GetX());
				camLED = 0;
				RangeLED = 0;
			}
			ds->SetDigitalOut(1, camLED % 2);
			ds->SetDigitalOut(2, (camLED >> 1) % 2);
			ds->SetDigitalOut(3, (camLED >> 2) % 2);
			ds->SetDigitalOut(4, RangeLED % 2);
			ds->SetDigitalOut(5, (RangeLED >> 1) % 2);
			ds->SetDigitalOut(6, (RangeLED >> 2) % 2);
		}
		else
		{
			if (!tracking)
			{
				two->Set(stick4->GetX());
			}
		}
	
	}
};

START_ROBOT_CLASS(RobotDemo);


